/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_

#include <chrono>
#include <memory>

#include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h"
#include "cartographer/mapping/internal/motion_filter.h"
#include "cartographer/mapping/internal/range_data_collator.h"
#include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer/mapping/proto/local_trajectory_builder_options_2d.pb.h"
#include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/internal/voxel_filter.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"

typedef std::unique_ptr<cartographer::transform::Rigid2d> Pose2dPtr;

namespace cartographer
{
    namespace mapping
    {
        /*
        以激光数据和位姿估计为输入， 使用Ceres库完成扫描匹配，输出位姿的观测值。一方面反馈给位姿估计器
        用于修正估计值，另一方面提供给运动滤波器(Motion Filter)用于判定机器人是否产生了运动。 如果产生
        了运动，则将此时的点云数据插入到当前正在维护的子图中。同时输出插入的结果，包括时间、位姿、子
        图、扫描数据等信息。
        */
        // Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
        // without loop closure.
        // TODO(gaschler): Add test for this class similar to the 3D test.
        class LocalTrajectoryBuilder2D
        {
        public:
            // 子图的插入结果
            struct InsertionResult
            {
                std::shared_ptr<const TrajectoryNode::Data> constant_data;      // 插入的节点
                std::vector<std::shared_ptr<const Submap2D>> insertion_submaps; // 被插入的子图
            };
            struct MatchingResult
            {
                common::Time time;             // 扫描匹配发生的时间
                transform::Rigid3d local_pose; // 局部地图坐标系下的位姿
                sensor::RangeData range_data_in_local;
                // 'nullptr' if dropped by the motion filter.
                // 子图插入结果,如果扫描匹配的结果被运动滤波器过滤了，那么字段insertion_result中
                // 记录的是一个空指针"nullptr"。
                std::unique_ptr<const InsertionResult> insertion_result;
            };
            // 猜想：机器人的运动轨迹是由一个个前后串联的节点(TrajectionNode)构成的，每当有新的扫描数据输入，
            // 该类都会先通过扫描匹配器生成一个MatchingResult的对象，但是它的insertion_result字段是留空的。
            // 只有当通过运动滤波器成功插入到子图中之后，才会将插入的结果记录下来， 于此同时子图就得到了更新。
            explicit LocalTrajectoryBuilder2D(
                const proto::LocalTrajectoryBuilderOptions2D &options,
                const std::vector<std::string> &expected_range_sensor_ids);

            ~LocalTrajectoryBuilder2D();

            LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D &) = delete;

            LocalTrajectoryBuilder2D &operator=(const LocalTrajectoryBuilder2D &) = delete;

            // Returns 'MatchingResult' when range data accumulation completed,
            // otherwise 'nullptr'. Range data must be approximately horizontal
            // for 2D SLAM. `TimedPointCloudData::time` is when the last point in
            // `range_data` was acquired, `TimedPointCloudData::ranges` contains the
            // relative time of point with respect to `TimedPointCloudData::time`.
            std::unique_ptr<MatchingResult> AddRangeData(const std::string &sensor_id,
                                                         const sensor::TimedPointCloudData &range_data);

            void AddImuData(const sensor::ImuData &imu_data);

            void AddOdometryData(const sensor::OdometryData &odometry_data);

            static void RegisterMetrics(metrics::FamilyFactory *family_factory);

        private:
            std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
                common::Time time, const sensor::RangeData &gravity_aligned_range_data,
                const transform::Rigid3d &gravity_alignment,
                const absl::optional<common::Duration> &sensor_duration);

            sensor::RangeData TransformToGravityAlignedFrameAndFilter(
                const transform::Rigid3f &transform_to_gravity_aligned_frame,
                const sensor::RangeData &range_data) const;

            std::unique_ptr<InsertionResult> InsertIntoSubmap(
                common::Time time, const sensor::RangeData &range_data_in_local,
                const sensor::PointCloud &filtered_aligned_cloud,
                const transform::Rigid3d &pose_estimate,
                const Eigen::Quaterniond &gravity_alignment);

            // Scan matches 'filtered_aligned_cloud' and returns the
            // observed pose, or nullptr on failure.
            Pose2dPtr ScanMatch(common::Time time, const transform::Rigid2d &pose_prediction,
                                const sensor::PointCloud &filtered_aligned_cloud);

            // Lazily constructs a PoseExtrapolator.
            void InitializeExtrapolator(common::Time time);

        private:
            int num_accumulated_ = 0; // 累积数据的数量
            const proto::LocalTrajectoryBuilderOptions2D options_;
            // 除了刚开始构建该对象的时候，只有一个子图(Submap2D)，其他时候它都维护着两个子图对象。一个子图用于进行扫描匹配，称为旧图。
            // 另一个子图被称为新图用于插入扫描数据。当新图中插入一定数量的数据完成了初始化操作之后，它就会被当作旧图，用于扫描匹配。
            // 对象active_submaps_将抛弃原来的旧图，并重新构建一个新图。两个submap之间有50%的重合，所以匹配的时候都用这个完成度
            // 大于50%的地图来进行匹配
            ActiveSubmaps2D active_submaps_; // 当前正在维护的子图
            MotionFilter motion_filter_;     // 运动滤波器，对位姿相关的数据进行降采样
            RangeDataCollator range_data_collator_;

            sensor::RangeData accumulated_range_data_; // 累积的扫描数据
            // 位姿估计器，用一段时间内的位姿数据估计线速度和角速度，进而预测运动
            std::unique_ptr<PoseExtrapolator> extrapolator_;
            // 使用Ceres库将扫描数据放置到地图中的扫描匹配器
            scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
            // 实时相关性分析的扫描匹配器，算法"Real-Time Correlative Scan Matching"的实现
            scan_matching::RealTimeCorrelativeScanMatcher2D real_time_correlative_scan_matcher_;

            absl::optional<common::Time> last_sensor_time_;
            absl::optional<double> last_thread_cpu_time_seconds_;
            absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
        };

    } // namespace mapping
} // namespace cartographer

#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_2D_H_
